//========================================================================
//  This software is free: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License Version 3,
//  as published by the Free Software Foundation.
//
//  This software is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public License
//  Version 3 in the file COPYING that came with this distribution.
//  If not, see <http://www.gnu.org/licenses/>.
//========================================================================
/*!
\file    kinect-audiosurf.cpp
\brief   Control Audiosurf using the Kinect
\author  Joydeep Biswas, (C) 2011
*/
//========================================================================

#include "stdafx.h"
#include "windows.h"
#include "math.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include "kinect_wrapper.h"
#include "geometry.h"

KinectWrapper kinect;
HWND audiosurf;

/// Coordinate frame conversion: new x is facing forward from kinect, y is to the left of kinect, z is up
#define GET_KINECT_COORDS(p) (p).z,(p).x,(p).y
void audiosurfControl(const NUI_SKELETON_DATA &skeleton);

void sendKey(WORD keyCode)
{
  /* 
  From MSDN:
  " Synthesizes a keystroke. The system can use such a synthesized keystroke to generate a WM_KEYUP or WM_KEYDOWN message.
  The keyboard driver's interrupt handler calls the keybd_event function. "
  */
  keybd_event(keyCode,MapVirtualKey(keyCode,MAPVK_VK_TO_VSC),0,0);
  Sleep(20);
  keybd_event(keyCode,MapVirtualKey(keyCode,MAPVK_VK_TO_VSC),KEYEVENTF_KEYUP,0);
}

/// Audiosurf keypress to switch to specified lane
void as_lane(int lane)
{
  if(lane<1 || lane>3)
    return;
  sendKey(0x30+lane);
}

/// Audiosurf keypress '1' to switch to lane 1
void as_lane1()
{
  sendKey(0x31);
}

/// Audiosurf keypress '2' to switch to lane 2
void as_lane2()
{
  sendKey(0x32);
}

/// Audiosurf keypress '3' to switch to lane 3
void as_lane3()
{
  sendKey(0x33);
}

///Keep track of modifier in use: 0=None, 1=left, 2=right
int lastModifier = 0;

/// Audiosurf Left Mouse Button Modifer
void as_modL()
{
  if(lastModifier==2)
    keybd_event(0x58,MapVirtualKey(0x58,MAPVK_VK_TO_VSC),KEYEVENTF_KEYUP,0);
  keybd_event(0x5A,MapVirtualKey(0x5A,MAPVK_VK_TO_VSC),0,0);
  lastModifier = 1;
}

/// Audiosurf Right Mouse Button Modifer
void as_modR()
{
  if(lastModifier==1)
    keybd_event(0x5A,MapVirtualKey(0x5A,MAPVK_VK_TO_VSC),KEYEVENTF_KEYUP,0);
  keybd_event(0x58,MapVirtualKey(0x58,MAPVK_VK_TO_VSC),0,0);
  lastModifier = 2;
}

/// Audiosurf Right Mouse Button Modifer
void as_modNone()
{
  if(lastModifier==1)
    keybd_event(0x5A,MapVirtualKey(0x5A,MAPVK_VK_TO_VSC),KEYEVENTF_KEYUP,0);
  else if(lastModifier==2)
    keybd_event(0x58,MapVirtualKey(0x58,MAPVK_VK_TO_VSC),KEYEVENTF_KEYUP,0);
  lastModifier = 0;
}

void testKeypresses()
{
  while(1){
    if(GetForegroundWindow()!=audiosurf){
      printf("Active window:%d audiosurf:%d\n",int(GetForegroundWindow()), int(audiosurf));
      Sleep(500);
      continue;
    }
    printf("Sent Keys\n");
    as_lane1();
    Sleep(500);
    as_lane3();
    Sleep(500);
    as_lane2();
    Sleep(1000);
  }
}

void labeledDepthImageCallback(BYTE* img)
{
}

void rgbImageCallback(BYTE* img)
{
}

void skeletonCallback(NUI_SKELETON_FRAME* skeletons)
{
  static const bool debug = true;
  static int lastUserNum = -1;

  bool userFound = false;
  bool userTracked = false;
  DWORD qualityFlags = 0;
  int userNum = lastUserNum;

  if(lastUserNum>=0){
    // See if the user still exists
    if(skeletons->SkeletonData[lastUserNum].eTrackingState == NUI_SKELETON_TRACKED && skeletons->SkeletonData[lastUserNum].dwQualityFlags==0){
      userTracked = true;
      userFound = true;
      qualityFlags = 0;
    }else{
      lastUserNum = -1; 
    }
  }
  bool userSwitched = lastUserNum!=userNum;

  for(int i=0; i<NUI_SKELETON_COUNT && !userTracked; i++){
    const NUI_SKELETON_DATA &skeleton = skeletons->SkeletonData[i];
    if(skeleton.eTrackingState == NUI_SKELETON_TRACKED && skeleton.dwQualityFlags==0){
      // Skeleton is being tracked, and is completely visible
      userFound = true;
      userTracked = true;
      qualityFlags = 0;
      userNum = i;
    }
    if(skeleton.eTrackingState == NUI_SKELETON_TRACKED){
      qualityFlags = skeleton.dwQualityFlags;
      userFound = true;
    }
  }
  
  // Can't do anything if the user's not obseravble now, can we?
  if(!userTracked){
    as_modNone(); //Neccesary to release all keys
    return;
  }
  
  const NUI_SKELETON_DATA &skeleton = skeletons->SkeletonData[userNum];
  lastUserNum = userNum;

  audiosurfControl(skeleton);
}

void audiosurfControl(const NUI_SKELETON_DATA &skeleton)
{
  // Parameters for hand gestures
  ///Maximum angle which the right forearm vector can make with the spine to count as a valid gesture
  static const double MaxRightForeArmAngle = RAD(70.0); 
  ///Maximum angle which the left forearm vector can make with the spine to count as a valid gesture
  static const double MaxLeftForeArmAngle = RAD(100.0); 
  /// Angle threshold for lane selection
  static const double FAThreshold = RAD(20.0); 
  /// Angle margin
  static const double FAMarginAngle = RAD(5.0);
  // Angle threshold for left hand to control modifiers
  static const double LAThreshold = RAD(20.0);
  
  static const bool debug = true;

  if(GetForegroundWindow()!=audiosurf)
    return;
  
  vector3d elbowR(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_RIGHT]));
  vector3d shoulderR(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT]));
  vector3d handR(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]));
  vector3d elbowL(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_LEFT]));
  vector3d shoulderL(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT]));
  vector3d handL(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT]));
  vector3d shoulderC(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_CENTER]));
  vector3d spine(GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SPINE]));

  vector3d spineVector = (shoulderC - spine).norm();
  vector3d forearmLVector = (handL - elbowL).norm();
  vector3d forearmRVector = (handR - elbowR).norm();
  
  // Extract non- depth components only
  vector2d spineVector2d(spineVector.y,spineVector.z);
  vector2d forearmLVector2d(forearmLVector.y,forearmLVector.z);
  vector2d forearmRVector2d(forearmRVector.y,forearmRVector.z);
  spineVector2d.normalize();
  forearmLVector2d.normalize();
  forearmRVector2d.normalize();

  double angleL = asin(forearmLVector2d.cross(spineVector2d));
  double angleR = asin(forearmRVector2d.cross(spineVector2d));
  bool validL = forearmLVector.dot(spineVector)>cos(MaxLeftForeArmAngle);
  bool validR = forearmRVector.dot(spineVector)>cos(MaxRightForeArmAngle);
  if(debug) printf("angleL(%d):%6.1f angleR(%d): %6.1f\n", validL?1:0, DEG(angleL), validR?1:0, DEG(angleR));

  static int lastLane = 2;
  static double lastAngle = 0.0;
  if(validR){
    if(angleR<-FAThreshold-FAMarginAngle){
      as_lane1();
    }else if(angleR>FAThreshold+FAMarginAngle){
      as_lane3();
    }else if(fabs(angleR)<FAThreshold-FAMarginAngle){
      as_lane2();
    }
    printf("la:%6.1f, ll:%d ",DEG(lastAngle), lastLane);
  }
  if(validL){
    if(angleL<-LAThreshold){
      as_modL();
    }else if(angleL>LAThreshold){
      as_modR();
    }else{
      as_modNone();
    }
  }else{
    as_modNone();
  }

}

int _tmain(int argc, _TCHAR* argv[])
{
  //Find Audiosurf window
  audiosurf = FindWindow(_T("START"),_T("Audiosurf"));
  if(audiosurf==NULL){
    printf("Audiosurf window not found!\n");
    return -1;
  }else{
    printf("Audiosurf HWND:%d\n",audiosurf);
  }

  // Start up Kinect polling thread
  printf("Connecting to Kinect...\n");
  int retval = kinect.connect(NULL,&labeledDepthImageCallback,&skeletonCallback,&rgbImageCallback);
  printf("%s\n",retval==0?"Success.":"Failure.");
  if(retval!=0)
    return retval;

  // Start Main loop
  while(1){
    //Get Data from Kinect
    kinect.poll();
    //Sleep for just a little while
    Sleep(5);
  }

  return 0;
}

